/**
 * Comprehensive BLDC motor control example using encoder and the DRV8302 board
 *
 * Using serial terminal user can send motor commands and configure the motor and FOC in real-time:
 * - configure PID controller constants
 * - change motion control loops
 * - monitor motor variabels
 * - set target values
 * - check all the configuration values
 *
 * check the https://docs.simplefoc.com for full list of motor commands
 *
 */
// v2.2.2版本，针对电流采样配置做了修改，代码中已做注释

#include <SimpleFOC.h>
#include <Arduino.h>
#include <FastLED.h>
#include "ESP32Tone.h"
#include "Motor_Init.h"
GBM5010_Motor_Angle_Control_Init GBM5010_motor_angle_control_init;//用于对电机的一些PID参数进行初始化，这样就不用在每个文件都改了，只需要改这个就好了
#define WS2812_1_DATA_PIN 4
#define WS2812_2_DATA_PIN 19
#define NUM_LEDS 1
#define LED_TYPE WS2812 // LED灯带型号
#define COLOR_ORDER GRB // RGB灯珠中红色、绿色、蓝色LED的排列顺序
#define BUZZER 15
#define LED_pin 2
uint8_t max_bright = 255; // LED亮度控制变量，可使用数值为 0 ～ 255， 数值越大则光带亮度越高
CRGB leds_1[NUM_LEDS];	  // 建立灯带leds
CRGB leds_2[NUM_LEDS];	  // 建立灯带leds
uint32_t frequency = 400000;
double init_p_sensor = 0;
double init_p_sensor1 = 0;
double compensation_motor = 0;
double compensation_motor1 = 0;
int commaPosition = 0; //存储还没有分离出来的字符串
double motor1_angle = 0, motor2_angle = 0;
// Motor instance
BLDCMotor motor = BLDCMotor(7);							// 11  必须设置为准确的极对数，浮点运算导致估算出的极对数有小数位
BLDCDriver3PWM driver = BLDCDriver3PWM(32, 14, 25, 12); // M1电机 //(INH_A, INH_B, INH_C, EN_GATE);

// magnetic sensor instance - SPI
// MagneticSensorSPI sensor = MagneticSensorSPI(AS5147_SPI, 5); //对于ESP32:CS=5,SCK=18,MISO=19,MOSI=23
// magnetic sensor instance - I2C
// SDA 21
// SCL 22
MagneticSensorI2C sensor = MagneticSensorI2C(AS5600_I2C);  //SPI接口和I2C接口，打开一个屏蔽一个即可

// DRV8302 board has 0.005Ohm shunt resistors and the gain of 12.22 V/V
InlineCurrentSense current_sense = InlineCurrentSense(0.01, 50.0, 39, 36);
// LowsideCurrentSense current_sense = LowsideCurrentSense(0.005, 10, 34, 35, 32); //(0.005f, 12.22f, IOUTA, IOUTB, IOUTC);

// commander interface
Commander command = Commander(Serial);
// void onMotor(char* cmd){ command.motor(&motor, cmd); }   //与studio交互的配置1共3处
void doTarget(char *cmd) { command.scalar(&motor.target, cmd); } //只接收目标值

//命令设置
float target_angle = 0;
// Commander command = Commander(Serial);
// void doTarget(char* cmd) { command.scalar(&target_angle, cmd); }
int pu[5] = {1000, 1000, 1500, 2000, 2500};
int delay_ms[5] = {100, 100, 300, 300, 300};
int notone_delay_ms[] = {100, 300, 100, 100, 100};
// pin:蜂鸣器引脚
void BUZZER_beep(int pin)
{
	for (int i = 2; i < 5; i++)
	{
		tone(pin, pu[i] - 1);
		delay(delay_ms[i]);
		noTone(pin);
		delay(notone_delay_ms[i]);
	}
	digitalWrite(pin, HIGH);
	delay(delay_ms[0]);
	digitalWrite(pin, LOW);
	delay(notone_delay_ms[0]);
	digitalWrite(pin, HIGH);
	delay(delay_ms[1]);
	digitalWrite(pin, LOW);
	delay(notone_delay_ms[1]);
}

uint8_t startIndex = 0;
//色板函数
void palette_color()
{
	startIndex++;
	// lED光带对象 灯珠数量 起始颜色序号 灯珠间色差 色板名称 亮度 颜色过渡效果(NOBLEND)
	fill_palette(leds_1, NUM_LEDS, startIndex, 8, LavaColors_p, 255, LINEARBLEND);
	fill_palette(leds_2, NUM_LEDS, startIndex, 8, CloudColors_p, 255, LINEARBLEND);
	FastLED.show();
}

void task_LED(void *pvParam) // 宣告任务LED闪烁
{
	/* 使用ledc通道1，设置频率为1kHz，分辨率为10位 */
	int freq = 1000;														  // 频率
	int channel = 0;														  // 通道
	int resolution = 8;														  // 分辨率
	ledcSetup(channel, freq, resolution);									  // 设置通道
	ledcAttachPin(LED_BUILTIN, channel);									  // 将通道与对应的引脚连接
	LEDS.addLeds<LED_TYPE, WS2812_1_DATA_PIN, COLOR_ORDER>(leds_1, NUM_LEDS); // 初始化灯带
	LEDS.addLeds<LED_TYPE, WS2812_2_DATA_PIN, COLOR_ORDER>(leds_2, NUM_LEDS); // 初始化灯带
	FastLED.setBrightness(max_bright);										  // 设置灯带亮度
	while (1)
	{
		palette_color();
		// 逐渐变亮
		for (int dutyCycle = 0; dutyCycle <= 255; dutyCycle = dutyCycle + 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}

		// 逐渐变暗
		for (int dutyCycle = 255; dutyCycle >= 0; dutyCycle = dutyCycle - 5)
		{
			ledcWrite(channel, dutyCycle); // 输出PWM
			vTaskDelay(pdMS_TO_TICKS(20));
		}
	}
}

void setup()
{
	xTaskCreate(task_LED, "task_LED", 1000, NULL, 1, NULL);
	pinMode(BUZZER, OUTPUT);
	BUZZER_beep(BUZZER);
	// initialise magnetic sensor hardware
	sensor.init();
	// link the motor to the sensor
	motor.linkSensor(&sensor);

	// driver config
	// power supply voltage [V]
	driver.voltage_power_supply = 24;
	driver.init();
	// link the motor and the driver
	motor.linkDriver(&driver);
	current_sense.linkDriver(&driver); //连接电流采样和驱动器  simpleFOC版本v2.2.2新增

	motor.voltage_sensor_align = 0.6; //零点校准对齐电压，默认为3，大功率电机设置小比如0.5-1,小功率设置大比如2-3,

	motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
	motor.torque_controller = TorqueControlType::foc_current; // voltage,dc_current,foc_current
	motor.controller = MotionControlType::velocity;			  // torque,velocity,angle
	motor.motion_downsample = 0;							  //控制motor.move()的执行频率，设置为0表示每次主循环都执行

	// velocity loop PID
	motor.PID_velocity.P = GBM5010_motor_angle_control_init.PID_velocity_P;; //
	motor.PID_velocity.I = GBM5010_motor_angle_control_init.PID_velocity_I;
	motor.PID_velocity.D = GBM5010_motor_angle_control_init.PID_velocity_D;
	// Low pass filtering time constant
	motor.LPF_velocity.Tf = GBM5010_motor_angle_control_init.LPF_velocity_Tf;		 // Tf设置小一点，配合爬升斜率设置，速度切换更平稳；如果不用爬升模式，Tf太小电机容易抖动，可设置Tf=0.02。
	motor.PID_velocity.output_ramp = GBM5010_motor_angle_control_init.PID_velocity_output_ramp; //速度爬升模式，限制斜率
	// angle loop PID
	motor.P_angle.P = GBM5010_motor_angle_control_init.P_angle_P;
	// Low pass filtering time constant
	motor.LPF_angle.Tf = GBM5010_motor_angle_control_init.LPF_angle_Tf;
	// current q loop PID
	motor.PID_current_q.P = 0.35;  // P参数大电机失控发出刺耳尖叫，5008航模电机P=0.35,I=0.001；云台电机P=1,I=0
	motor.PID_current_q.I = 0.001; //航模电机不加积分堵转电流上不去，0.001-0.005之间好像没有区别
	// Low pass filtering time constant
	motor.LPF_current_q.Tf = 0.02;
	// current d loop PID
	motor.PID_current_d.P = 0.35; //与q轴保持一致
	motor.PID_current_d.I = 0.001;
	// Low pass filtering time constant
	motor.LPF_current_d.Tf = 0.02;

	// Limits
	motor.velocity_limit = 20.0; // 100 rad/s velocity limit
	motor.voltage_limit = 6;	 // 12 Volt limit, 限制SVPWM的输出电压，角度模式和速度模式起作用，力矩模式不起作用,
	motor.current_limit = 50;	 // 2 Amp current limit, 速度模式起限制电流作用并限制最大速度

	// use monitoring with serial for motor init
	// monitoring port
	Serial.begin(115200);
	// comment out if not needed
	motor.useMonitoring(Serial);
	motor.monitor_variables = _MON_VEL; //_MON_CURR_Q | _MON_CURR_D; // monitor the two currents d and q
	motor.monitor_downsample = 0;		//设置为0表示不上传参数。如果不设置为0，默认上传4个参数

	// initialise motor
	motor.init();

	current_sense.init(); // current_sense.driverSync(&driver); //已经包含在了init函数中，所以屏蔽。simpleFOC版本v2.2.2
	// current_sense.skip_align = true;    //对于ESP32drive可以跳过电流检测，同时增益乘以-1，也就是把这四行都打开，
	// current_sense.gain_a *=-1;  //在之前的simpleFOC版本中，是不需要乘以-1的，v2.2.2修改了底层代码，所以必须乘以-1
	// current_sense.gain_b *=-1;
	// current_sense.gain_c *=-1;  //这四行也可以都屏蔽了，通过代码检测电流增益，
	motor.linkCurrentSense(&current_sense);

	// align encoder and start FOC
	motor.initFOC(); //(0.73, CW)  先上电校准一次，获得参数后填入，重新下载代码，以后可跳过零点检测

	// set the inital target value
	motor.target = 0;

	// define the motor id
	// command.add('M', onMotor, (char *)"motor");   //与studio交互的配置2共3处
	command.add('T', doTarget, (char *)"target"); //接收电机的运动指令

	Serial.println("Motor ready,Current Control Low_side Mode.");

	_delay(1000);
}

void loop()
{
	// iterative setting FOC phase voltage
	motor.loopFOC();

	// iterative function setting the outter loop target
	motor.move();

	// monitoring the state variables
	// motor.monitor();   //会影响程序执行速度，与studio交互的配置3共3处

	// user communication
	command.run();
}
